/* 
* @Author: obsidian
* @Date:   2015-12-13 22:08:31
* @Last Modified by:   obsidian
* @Last Modified time: 2016-06-04 09:37:44
*/
#include "ledboard.h"
#include "driver/can/can.h"
#include "module/GPS/GPS.h"
#include "driver/math/inc/math.h"
#include "driver/led/inc/led.h"
#include "math.h"
#include "stm32f10x_usart.h"
#include "control/GPSpath/GPSpath.h"
#include "control/GPSpath/pointpath.h"
#include "control/car.h"

static float ledboard_getdiserr(void);
static void  ledboard_calibrate(void);
static void UART2_init(void);
static void change_threshold(unsigned char id);
static void ledboard_clear(void);

float angle_offset = 0;

struct Ledboard ledboard = 
{
        .init               = UART2_init,//
        .offset             = 2.5,
        .getdiserr          = ledboard_getdiserr,
        .calibrate          = ledboard_calibrate,
        .ratio              = 1,
        .change_threshold   = change_threshold,
	    .clear              = ledboard_clear,
};


static float ledboard_getdiserr(void)
{
    extern int start_flag;
    static int data_flag = 1;
    static int temp_offs;
    if(start_flag)
    {
        temp_offs = ledboard.offset;
        ledboard.offset = 0;
        data_flag++;
        if(temp_offs != 4 && data_flag != 2) start_flag = 0;      
    }
    
	float temp_offset = ledboard.offset;
	static float last_err = 0;
	static float last_offset = 0;
    

	if(ledboard.offset == 0xff){return last_err;}
	if(ledboard.offset == 0)
	{
		last_err = 0;
		return last_err;
	}
	if(ledboard.offset>10)
	{
		temp_offset=ledboard.offset-10;
	}
	float  tmp = 0;

    if (temp_offset == 3 || temp_offset == 2)
    {
        tmp = (temp_offset - 2.5) * 68.0/3.0;
        tmp = (tmp + 4 * last_err)/4.5 ;//3 
    }
    if (temp_offset == 4)
    {
        tmp = (temp_offset - 2.5) * 75/3.0;
        tmp = (tmp + 4 * last_err)/4.5 ;//3 
    }
    if(temp_offset == 1)
    {
        tmp = (temp_offset - 2.5) * 69/3.0;
        tmp = (tmp + 3 * last_err)/3.5 ;//3 
    }        
    if(ledboard.offset == 0)
    {
        TIM4->CCR3 = 6300;
    }
    if( ledboard.offset == 1 || ledboard.offset == 11 || ledboard.offset == 14)
    {
        TIM4->CCR3 = 10000;
    }
    if( ledboard.offset == 4 )   
    {
        TIM4->CCR3 = 9800;
    }
    if(ledboard.offset == 2 || ledboard.offset == 3)
    {
        TIM4->CCR3 = 6000;
    }
    if( ledboard.offset == 12 || ledboard.offset == 13)
    {
        TIM4->CCR3 = 10000;
    }
	last_err = tmp;
	last_offset = ledboard.offset;
	return tmp;
}

static char calibrate_flag = 0;
static char calibrate_flag2 = 0;
static char flag0 = 0;
static char flag1 = 0;
static char flag2 = 0;
static char flag3 = 0;


static void  ledboard_calibrate(void)
{
     if(ledboard.status == 0){return;}

//     if( flag1 == 1 &&gps.rear_point.y > 2420 && gps.rear_point.y < 2990 )
//     {
//             if(ledboard.offset > 60 || ledboard.offset < -60)
//             {
//                     //angle_offset = ledboard.offset/sqrt( pow(gps.front_point.y-gps.start_point.y,2) + pow(gps.front_point.x,2) );
//                    // gps.base_radian  -= angle_offset;//ÈçÏòÓÒÆ«angle_offsetÎª¸º  base_radianÔö´ó  ËùÒÔ×îÖÕ½Ç¶ÈÒ²Ôö´ó ÓëÊµ¼ÊÓÒÆ«Ïà·û
//                     //gps.rear_point.x -=ledboard.offset*1.5;
//                     //LED3_off;
//                     //rotate((float *)&gps.rear_point, angle_offset);//
//                     flag1 = 0;
//             }
//     }
//     else if(flag2 == 1 && gps.front_point.y > 4285 && gps.front_point.y < 4870 )
//     {
//             angle_offset = ledboard.offset/sqrt( pow(gps.front_point.y-gps.start_point.y,2) + pow(gps.front_point.x,2) );
//             gps.base_radian  -= angle_offset;
//             //gps.rear_point.x -=ledboard.offset*1.5;
//             LED3_off;
//             rotate((float *)&gps.rear_point, angle_offset);
//             flag2 = 0;
//     }
//     if(flag0 == 0 && gps.front_point.y > 2000 && gps.front_point.y < 2100 )
//     {
//         if(ledboard.getdiserr() > 40 || ledboard.getdiserr() < -40){return;}
//         if(ledboard.offset == 255){return;}
//         gps.rear_point.x = path[gpspath.current_index].point.x - gps.car_len * sin(gps.radian);
//         gps.rear_point.y = path[gpspath.current_index].point.y - gps.car_len * cos(gps.radian);

//         flag0 = 1;
//     }

//     if(flag1 == 0 && gps.front_point.y > 3700 && gps.front_point.y < 3800 )
//     {
//         if(ledboard.getdiserr() > 70 || ledboard.getdiserr() < -70){return;}
//         if(ledboard.offset == 255){return;}
//         float dis = 0; 
//  		dis = get_point2point_length(gps.front_point, judge_direc(gpspath.current_index).point);
//  		if(gps.front_point.x > judge_direc(gpspath.current_index).point.x){dis = -dis;}
// 		dis -= ledboard.getdiserr();
// 		gps.base_radian += dis / gps.front_point.y;
// 		gps.rear_point.x = judge_direc(gpspath.current_index).point.x - gps.car_len * sin(gps.radian) - ledboard.getdiserr() * cos(gps.radian);
// 		gps.rear_point.y = judge_direc(gpspath.current_index).point.y - gps.car_len * cos(gps.radian) + gpspath.offset_y + ledboard.getdiserr() * sin(gps.radian);			

//         flag1 = 1;
//     }

    if(flag2 == 0 && gps.front_point.y > 5900 && gps.front_point.y < 6000 )
    {
		float diserr = ledboard.getdiserr();
        if(diserr > 80 || diserr < -80){return;}
        if(ledboard.offset == 255){return;}
        float dis = 0; 
 		dis = get_point2point_length(gps.front_point, judge_direc(gpspath.current_index).point);
 		if(gps.front_point.x > judge_direc(gpspath.current_index).point.x){dis = -dis;}
		dis -= diserr;
		gps.base_radian += dis / gps.front_point.y;
		gps.rear_point.x = judge_direc(gpspath.current_index).point.x - gps.car_len * sin(gps.radian) - diserr * cos(gps.radian);
		gps.rear_point.y = judge_direc(gpspath.current_index).point.y - gps.car_len * cos(gps.radian) + gpspath.offset_y + diserr * sin(gps.radian);			

        flag2 = 1;
    }

    if(flag3 == 0 && gpspath.current_index > 1550 && gpspath.current_index < 1660 )//过直角弯之后的橙色区域
    {
        if(ledboard.getdiserr() > 100 || ledboard.getdiserr() < -100){return;}
        if(ledboard.offset == 255){return;}
		if(gps.direction == 2)
		{
			if(gps.angle < 55){return;}
		}
		else if(gps.direction == 0)
		{
			if(gps.angle > -55){return;}
		}
        gps.rear_point.y = 6579.0 - ledboard.getdiserr() * sin(-gps.radian) - (gps.led_dis)* cos(-gps.radian) + 10;

        flag3 = 0;
		LED2_on;
    }



    float distance      =   0;
    float theta         =   0;
    if(gps.front_point.y >= 9400 && gps.front_point.y < 9963 && !calibrate_flag)
    {
            gps.rear_point.x = judge_direc(gpspath.current_index).point.x - gps.car_len * sin(gps.radian);
            gps.rear_point.y = judge_direc(gpspath.current_index).point.y - gps.car_len * cos(gps.radian);

            calibrate_flag = 1;
    }
    if(gps.front_point.y >= 7703 && gps.front_point.y < 7900 && !calibrate_flag2)
    {
            gps.rear_point.x = judge_direc(gpspath.current_index).point.x - gps.car_len * sin(gps.radian);
            gps.rear_point.y = judge_direc(gpspath.current_index).point.y - gps.car_len * cos(gps.radian);

            calibrate_flag2 = 1;
    }
}

static void ledboard_clear(void)
{
	calibrate_flag = 0;
	calibrate_flag2 = 0;
	flag0 = 0;
	flag1 = 0;
	flag2 = 0;
	flag3 = 0;
	ledboard.change_flag0 = 0;
	ledboard.change_flag1 = 0;
}


//USART2_TX:PD5 USART2_RX:PD6
void UART2_init(void)
{
        GPIO_InitTypeDef GPIO_InitStructure;
        USART_InitTypeDef USART_InitStructure;
        NVIC_InitTypeDef NVIC_InitStructure;

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD|RCC_APB2Periph_AFIO, ENABLE);   //??USART3 GPIO
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
        GPIO_PinRemapConfig(GPIO_Remap_USART2,ENABLE);

        USART_DeInit(USART2);  //
        //USART3_TX   PD5
        GPIO_InitStructure.GPIO_Pin     = GPIO_Pin_5; //
        GPIO_InitStructure.GPIO_Speed   = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode    = GPIO_Mode_AF_PP; //
        GPIO_Init(GPIOD, &GPIO_InitStructure); //

        //USART3_RX   PD6
        GPIO_InitStructure.GPIO_Pin     = GPIO_Pin_6;
        GPIO_InitStructure.GPIO_Mode    = GPIO_Mode_IPU;//
        GPIO_Init(GPIOD, &GPIO_InitStructure);  //

        NVIC_InitStructure.NVIC_IRQChannel                   = USART2_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =3 ;//
        NVIC_InitStructure.NVIC_IRQChannelSubPriority        = 3;       //
        NVIC_InitStructure.NVIC_IRQChannelCmd                = ENABLE;          //
        NVIC_Init(&NVIC_InitStructure);  //

        //USART_InitStructure.USART_BaudRate      = bound;//;
        USART_InitStructure.USART_BaudRate      = 115200;//;		
        USART_InitStructure.USART_WordLength    = USART_WordLength_8b;//
        USART_InitStructure.USART_StopBits      = USART_StopBits_1;//
        USART_InitStructure.USART_Parity        = USART_Parity_No;//
        USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//
        USART_InitStructure.USART_Mode          = USART_Mode_Rx | USART_Mode_Tx; //

        USART_Init(USART2, &USART_InitStructure); 
        USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//
        USART_Cmd(USART2, ENABLE);                    // 
}


// !!!!!!ÔÚÖÐ¶ÏÖÐÉèÖÃ±êÖ¾Î»½øÐÐ·¢ËÍ
// ¾­²âÊÔ  Ã¿ÃëÄÜ¹»¸üÐÂ200´Î×óÓÒ  ¼´5msÒ»´Î 
static signed short number_to_dis(u8 number);
signed short int error_dis_last[5]={11,11,11,11,11};
signed short int error_dis_average = 0;
static unsigned char count = 0;
signed short error_dis = 0;
void USART2_IRQHandler(void)
{
	unsigned char i = 0;
	u8 res = 0;
	if(count%50 == 0)
	{
	       LED4_on;		
	}
	if(count%100 == 0)
	{
                   LED4_off;	
	       count = 0;
	}
	
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)  
	{
            	       res =USART_ReceiveData(USART2);//(USART1->DR);	//¶ÁÈ¡½ÓÊÕµ½µÄÊý¾Ý
            	       error_dis = number_to_dis(res);
            	       error_dis_average = (error_dis_last[0]+error_dis_last[1]+error_dis_last[2]+error_dis_last[3]+error_dis)/5.0;

            	       for( i = 0;i<=3;i++)
            	       {
            		      error_dis_last[i] = error_dis_last[i+1];
            	       }
            	       error_dis_last[4] = error_dis;
            	       ledboard.offset = error_dis_average;
	}
	count++;
	USART_ClearITPendingBit(USART2, USART_IT_RXNE);  
}

signed short number_to_dis(u8 number)
{
        if(number == 11) return 0;
        else
        {
                return ( number - 11 )*131/21.0;
        }
}

void change_threshold(unsigned char id)
{
		int i;
    CanTxMsg TxM;
    TxM.RTR = CAN_RTR_Data;
    TxM.IDE = CAN_Id_Standard;
    TxM.DLC = 1;
    TxM.StdId = 0xc1;
	for( i=0;i<8;i++)
	{
		TxM.Data[i] = 0;
	}
    TxM.Data[0] = id;
    CAN_Transmit(CAN1,&TxM);    
}
